From 9bdcb1070f65e115cbe3871cf4ad36fdc6f36429 Mon Sep 17 00:00:00 2001
From: Merry <git@mary.rs>
Date: Fri, 10 Feb 2023 20:12:58 +0000
Subject: biquad_filter: Fix rounding in ApplyBiquadFilterInt

---
 .../renderer/command/effect/biquad_filter.cpp      | 32 ++++++++--------------
 src/audio_core/renderer/voice/voice_state.h        |  8 +++---
 2 files changed, 16 insertions(+), 24 deletions(-)

diff --git a/src/audio_core/renderer/command/effect/biquad_filter.cpp b/src/audio_core/renderer/command/effect/biquad_filter.cpp
index edb30ce72..52f775bfa 100644
--- a/src/audio_core/renderer/command/effect/biquad_filter.cpp
+++ b/src/audio_core/renderer/command/effect/biquad_filter.cpp
@@ -4,6 +4,7 @@
 #include "audio_core/renderer/adsp/command_list_processor.h"
 #include "audio_core/renderer/command/effect/biquad_filter.h"
 #include "audio_core/renderer/voice/voice_state.h"
+#include "common/bit_cast.h"
 
 namespace AudioCore::AudioRenderer {
 /**
@@ -26,8 +27,8 @@ void ApplyBiquadFilterFloat(std::span<s32> output, std::span<const s32> input,
                          Common::FixedPoint<50, 14>::from_base(b_[2]).to_double()};
     std::array<f64, 2> a{Common::FixedPoint<50, 14>::from_base(a_[0]).to_double(),
                          Common::FixedPoint<50, 14>::from_base(a_[1]).to_double()};
-    std::array<f64, 4> s{state.s0.to_double(), state.s1.to_double(), state.s2.to_double(),
-                         state.s3.to_double()};
+    std::array<f64, 4> s{Common::BitCast<f64>(state.s0), Common::BitCast<f64>(state.s1),
+                         Common::BitCast<f64>(state.s2), Common::BitCast<f64>(state.s3)};
 
     for (u32 i = 0; i < sample_count; i++) {
         f64 in_sample{static_cast<f64>(input[i])};
@@ -41,10 +42,10 @@ void ApplyBiquadFilterFloat(std::span<s32> output, std::span<const s32> input,
         s[2] = sample;
     }
 
-    state.s0 = s[0];
-    state.s1 = s[1];
-    state.s2 = s[2];
-    state.s3 = s[3];
+    state.s0 = Common::BitCast<s64>(s[0]);
+    state.s1 = Common::BitCast<s64>(s[1]);
+    state.s2 = Common::BitCast<s64>(s[2]);
+    state.s3 = Common::BitCast<s64>(s[3]);
 }
 
 /**
@@ -58,29 +59,20 @@ void ApplyBiquadFilterFloat(std::span<s32> output, std::span<const s32> input,
  * @param sample_count - Number of samples to process.
  */
 static void ApplyBiquadFilterInt(std::span<s32> output, std::span<const s32> input,
-                                 std::array<s16, 3>& b_, std::array<s16, 2>& a_,
+                                 std::array<s16, 3>& b, std::array<s16, 2>& a,
                                  VoiceState::BiquadFilterState& state, const u32 sample_count) {
     constexpr s64 min{std::numeric_limits<s32>::min()};
     constexpr s64 max{std::numeric_limits<s32>::max()};
-    std::array<Common::FixedPoint<50, 14>, 3> b{
-        Common::FixedPoint<50, 14>::from_base(b_[0]),
-        Common::FixedPoint<50, 14>::from_base(b_[1]),
-        Common::FixedPoint<50, 14>::from_base(b_[2]),
-    };
-    std::array<Common::FixedPoint<50, 14>, 3> a{
-        Common::FixedPoint<50, 14>::from_base(a_[0]),
-        Common::FixedPoint<50, 14>::from_base(a_[1]),
-    };
 
     for (u32 i = 0; i < sample_count; i++) {
-        s64 in_sample{input[i]};
-        auto sample{in_sample * b[0] + state.s0};
-        const auto out_sample{std::clamp(sample.to_long(), min, max)};
+        const s64 in_sample{input[i]};
+        const s64 sample{in_sample * b[0] + state.s0};
+        const s64 out_sample{std::clamp<s64>((sample + (1 << 13)) >> 14, min, max)};
 
         output[i] = static_cast<s32>(out_sample);
 
         state.s0 = state.s1 + b[1] * in_sample + a[0] * out_sample;
-        state.s1 = 0 + b[2] * in_sample + a[1] * out_sample;
+        state.s1 = b[2] * in_sample + a[1] * out_sample;
     }
 }
 
diff --git a/src/audio_core/renderer/voice/voice_state.h b/src/audio_core/renderer/voice/voice_state.h
index d5497e2fb..ce947233f 100644
--- a/src/audio_core/renderer/voice/voice_state.h
+++ b/src/audio_core/renderer/voice/voice_state.h
@@ -19,10 +19,10 @@ struct VoiceState {
      * State of the voice's biquad filter.
      */
     struct BiquadFilterState {
-        Common::FixedPoint<50, 14> s0;
-        Common::FixedPoint<50, 14> s1;
-        Common::FixedPoint<50, 14> s2;
-        Common::FixedPoint<50, 14> s3;
+        s64 s0;
+        s64 s1;
+        s64 s2;
+        s64 s3;
     };
 
     /**
-- 
cgit v1.2.3